﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Joints/b2DistanceJoint.h
// Revision   : r82
// Change-Date: 2010-04-04
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Joints/b2DistanceJoint.cpp
// Revision   : r28
// Change-Date: 2009-11-23
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    ///<summary>
    ///Distance joint definition. This requires defining an 
    ///anchor point on both bodies and the non-zero length of the 
    ///distance joint. The definition uses local anchor points 
    ///so that the initial configuration can violate the constraint 
    ///slightly. This helps when saving and loading a game.
    ///</summary>
    ///<remarks>Do not use a zero or short length.</remarks>
    public class DistanceJointDef : JointDef
    {
	    public DistanceJointDef()
            : base()
	    {
		    type = JointType.DISTANCE;
		    length = 1.0f;
	    }

        ///<summary>
	    /// Initialize the bodies, anchors, and length using the world
	    /// anchors.
        ///</summary>
	    public void Initialize(Body body1, Body body2, Vector2 anchor1, Vector2 anchor2)
        {
	        this.body1 = body1;
	        this.body2 = body2;
	        localAnchor1 = body1.GetLocalPoint(anchor1);
	        localAnchor2 = body2.GetLocalPoint(anchor2);
	        Vector2 d = anchor2 - anchor1;
	        length = d.Length();
        }

        ///<summary>
	    /// The local anchor point relative to body1's origin.
        ///</summary>
	    public Vector2 localAnchor1;

        ///<summary>
	    /// The local anchor point relative to body2's origin.
        ///</summary>
	    public Vector2 localAnchor2;

	    /// The natural length between the anchor points.
	    public float length;

	    /// The mass-spring-damper frequency in Hertz.
	    public float frequencyHz;

	    /// The damping ratio. 0 = no damping, 1 = critical damping.
	    public float dampingRatio;
    };

    /// <summary>A distance joint constrains two points on two bodies
    /// to remain at a fixed distance from each other. You can view
    /// this as a massless, rigid rod.</summary>
    public class DistanceJoint : Joint
    {
	    public override Vector2 GetAnchorA()
        {
            return m_body1.GetWorldPoint(ref m_localAnchor1);
        }

	    public override Vector2 GetAnchorB()
        {
            return m_body2.GetWorldPoint(ref m_localAnchor2);
        }

        /// <summary>
        /// Get the reaction force given the inverse time step. Unit is N.
        /// </summary>
	    public override Vector2 GetReactionForce(float inv_dt)
        {
	        Vector2 F = (inv_dt * m_impulse) * m_u;
	        return F;
        }

        /// <summary>
        /// Get the reaction torque given the inverse time step. Unit is N*m. This is always zero for distance joint.
        /// </summary>
        public override float GetReactionTorque(float inv_dt)
        {
            //B2_NOT_USED(inv_dt);
            return 0.0f;
        }

        /// <summary>
        /// Set/get the natural length.
        /// Manipulating the length can lead to non-physical behavior when the frequency is zero.
        /// </summary>
        public void SetLength(float length)
        {
            m_length = length;
        }

        public float GetLength()
        {
            return m_length;
        }

        public void SetFrequency(float hz)
        {
            m_frequencyHz = hz;
        }

        public float GetFrequency()
        {
            return m_frequencyHz;
        }

        public void SetDampingRatio(float ratio)
        {
            m_dampingRatio = ratio;
        }

        public float GetDampingRatio()
        {
            return m_dampingRatio;
        }

	    //--------------- Internals Below -------------------

	    public DistanceJoint(DistanceJointDef def)
            : base(def)
        {
	        m_localAnchor1 = def.localAnchor1;
	        m_localAnchor2 = def.localAnchor2;
	        m_length = def.length;
	        m_frequencyHz = def.frequencyHz;
	        m_dampingRatio = def.dampingRatio;
	        m_impulse = 0.0f;
	        m_gamma = 0.0f;
	        m_bias = 0.0f;
        }

	    internal override void InitVelocityConstraints(TimeStep step)
        {
	        Body b1 = m_body1;
	        Body b2 = m_body2;

	        // Compute the effective mass matrix.
	        Vector2 r1 = Math.Mul(b1.Transform.R, m_localAnchor1 - b1.LocalCenter);
	        Vector2 r2 = Math.Mul(b2.Transform.R, m_localAnchor2 - b2.LocalCenter);
	        m_u = b2.m_sweep.c + r2 - b1.m_sweep.c - r1;

	        // Handle singularity.
	        float length = m_u.Length();
	        if (length > PhysicSettings.LinearSlop)
	        {
		        m_u *= 1.0f / length;
	        }
	        else
	        {
		        m_u = Vector2.Zero;
	        }

	        float cr1u = Vector2Extensions.Cross(r1, m_u);
            float cr2u = Vector2Extensions.Cross(r2, m_u);
	        float invMass = b1.m_invMass + b1.m_invI * cr1u * cr1u + b2.m_invMass + b2.m_invI * cr2u * cr2u;

            m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

	        if (m_frequencyHz > 0.0f)
	        {
		        float C = length - m_length;

		        // Frequency
		        float omega = 2.0f * PhysicSettings.b2_pi * m_frequencyHz;

		        // Damping coefficient
		        float d = 2.0f * m_mass * m_dampingRatio * omega;

		        // Spring stiffness
		        float k = m_mass * omega * omega;

		        // magic formulas
                m_gamma = step.dt * (d + step.dt * k);
                m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
		        m_bias = C * step.dt * k * m_gamma;

                m_mass = invMass + m_gamma;
                m_mass = m_mass != 0.0f ? 1.0f / m_mass : 0.0f;
	        }

	        if (step.warmStarting)
	        {
		        // Scale the impulse to support a variable time step.
		        m_impulse *= step.dtRatio;

		        Vector2 P = m_impulse * m_u;
		        b1.m_linearVelocity -= b1.m_invMass * P;
                b1.m_angularVelocity -= b1.m_invI * Vector2Extensions.Cross(r1, P);
		        b2.m_linearVelocity += b2.m_invMass * P;
                b2.m_angularVelocity += b2.m_invI * Vector2Extensions.Cross(r2, P);
	        }
	        else
	        {
		        m_impulse = 0.0f;
	        }
        }

	    internal override void SolveVelocityConstraints(TimeStep step)
        {
	        //B2_NOT_USED(step);

	        Body b1 = m_body1;
	        Body b2 = m_body2;

	        Vector2 r1 = Math.Mul(b1.Transform.R, m_localAnchor1 - b1.LocalCenter);
	        Vector2 r2 = Math.Mul(b2.Transform.R, m_localAnchor2 - b2.LocalCenter);

	        // Cdot = dot(u, v + cross(w, r))
            Vector2 v1 = b1.m_linearVelocity + Vector2Extensions.Cross(b1.m_angularVelocity, r1);
            Vector2 v2 = b2.m_linearVelocity + Vector2Extensions.Cross(b2.m_angularVelocity, r2);
	        float Cdot = Vector2.Dot(m_u, v2 - v1);

	        float impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
	        m_impulse += impulse;

	        Vector2 P = impulse * m_u;
	        b1.m_linearVelocity -= b1.m_invMass * P;
            b1.m_angularVelocity -= b1.m_invI * Vector2Extensions.Cross(r1, P);
	        b2.m_linearVelocity += b2.m_invMass * P;
            b2.m_angularVelocity += b2.m_invI * Vector2Extensions.Cross(r2, P);
        }
	    
        internal override bool SolvePositionConstraints(float baumgarte)
        {
	        //B2_NOT_USED(baumgarte);

	        if (m_frequencyHz > 0.0f)
	        {
		        // There is no position correction for soft distance constraints.
		        return true;
	        }

	        Body b1 = m_body1;
	        Body b2 = m_body2;

	        Vector2 r1 = Math.Mul(b1.Transform.R, m_localAnchor1 - b1.LocalCenter);
	        Vector2 r2 = Math.Mul(b2.Transform.R, m_localAnchor2 - b2.LocalCenter);

	        Vector2 d = b2.m_sweep.c + r2 - b1.m_sweep.c - r1;
            
            float length = d.Length();
            d.Normalize();
            
            float C = length - m_length;
	        C = MathHelper.Clamp(C, -PhysicSettings.MaxLinearCorrection, PhysicSettings.MaxLinearCorrection);

	        float impulse = -m_mass * C;
	        m_u = d;
	        Vector2 P = impulse * m_u;

	        b1.m_sweep.c -= b1.m_invMass * P;
	        b1.m_sweep.a -= b1.m_invI * Vector2Extensions.Cross(r1, P);
	        b2.m_sweep.c += b2.m_invMass * P;
            b2.m_sweep.a += b2.m_invI * Vector2Extensions.Cross(r2, P);

	        b1.SynchronizeTransform();
	        b2.SynchronizeTransform();

	        return System.Math.Abs(C) < PhysicSettings.LinearSlop;
        }

        public Vector2 m_localAnchor1;
        public Vector2 m_localAnchor2;
        public Vector2 m_u;
        public float m_frequencyHz;
        public float m_dampingRatio;
        public float m_gamma;
        public float m_bias;
        public float m_impulse;
        public float m_mass;
        public float m_length;
    };
}
